#ifndef SANDBOX_SDK_MATH_MATRIX4F_HPP
#define SANDBOX_SDK_MATH_MATRIX4F_HPP

#include "vector4f.hpp"
#include <type_traits>
namespace sandbox
{
	namespace sdk
	{
		ALIGN_BEG(64) struct Matrix4f {
			Matrix4f(){}
			constexpr Matrix4f(
					 const Vector4f  cw,
					 const Vector4f  cx,
					 const Vector4f  cy,
					 const Vector4f  cz)
			{c[0]=cw;c[1]=cx;c[2]=cy;c[3]=cz;}
			constexpr Matrix4f(const Vector4f* cc)
			{for(int i=0;i<4;++i)c[i]=cc[i];}
			
			Vector4f operator [](int i)const{return Vector4f(c[i]);}	
			Matrix4f& operator *=(const Matrix4f& mat)
			{
				ALIGN_BEG(64) __m128 ALIGN_END(64) cc[4];
				for(int i=0;i<4;++i)
				{
					cc[i]=_mm_add_ps(
						_mm_add_ps(
							_mm_mul_ps(c[0],::sandbox::sdk::detail::shuffle<__m128, 0>(mac.c[i])),
							_mm_mul_ps(c[1],::sandbox::sdk::detail::shuffle<__m128, 1>(mac.c[i]))
						),
						_mm_add_ps(
							_mm_mul_ps(c[2],::sandbox::sdk::detail::shuffle<__m128, 2>(mac.c[i])),
							_mm_mul_ps(c[3],::sandbox::sdk::detail::shuffle<__m128, 3>(mac.c[i])),
						));
				}
				for(int i=0;i<4;++i)cc[i]=c[i];
				return *this;
			}
			__m128 c[4];
		} ALIGN_END(64);

		static_assert(std::is_pod<Matrix4f>::value==true);
		static_assert(sizeof(Matrix4f)==64);

		const Matrix4f operator *(const Matrix4f& a, const Matrix4f& b)
		{
			Matrix4f out;
			for(int i=0;i<4;++i)
			{
				out.c[i]=_mm_add_ps(
					_mm_add_ps(
						_mm_mul_ps(a.c[0],::sandbox::sdk::detail::shuffle<__m128, 0>(b.c[i])),
						_mm_mul_ps(a.c[1],::sandbox::sdk::detail::shuffle<__m128, 1>(b.c[i]))
					),
					_mm_add_ps(
						_mm_mul_ps(a.c[2],::sandbox::sdk::detail::shuffle<__m128, 2>(b.c[i])),
						_mm_mul_ps(a.c[3],::sandbox::sdk::detail::shuffle<__m128, 3>(b.c[i])),
					));
			}
			return out;
		}	
		const Vector4f operator *(const Matrix4f& a, const Vector4f  b)
		{
			return Vector4f(_mm_add_ps(
				_mm_add_ps(
					_mm_mul_ps(a.c[0],::sandbox::sdk::detail::shuffle<__m128, 0>(b.value)),
					_mm_mul_ps(a.c[1],::sandbox::sdk::detail::shuffle<__m128, 1>(b.value))
				),
				_mm_add_ps(
					_mm_mul_ps(a.c[2],::sandbox::sdk::detail::shuffle<__m128, 2>(b.value)),
					_mm_mul_ps(a.c[3],::sandbox::sdk::detail::shuffle<__m128, 3>(b.value)),
				));
		}
		
		const Vector4f operator %(const Matrix4f& a, const Vector4f  b)
		{
			return Vector4f(_mm_add_ps(
				_mm_mul_ps(a.c[1],::sandbox::sdk::detail::shuffle<__m128, 1>(b.value))
				_mm_add_ps(
					_mm_mul_ps(a.c[2],::sandbox::sdk::detail::shuffle<__m128, 2>(b.value)),
					_mm_mul_ps(a.c[3],::sandbox::sdk::detail::shuffle<__m128, 3>(b.value)),
				));
		}
		
		const Vector4f operator *(const Vector4f  b,const Matrix4f& a)
		{
			__m128 v[4];
			for(int i=0;i<4;++i)v[i]=_mm_mul_ps(a[i].value,b.value);
#ifdef __SSE3__
			__m128 w0=_mm_hadd_ps(v[0],v[1]);
			__m128 w1=_mm_hadd_ps(v[2],v[3]);
			return Vector4f(_mm_hadd_ps(w0,w1));
#else			
			__m128 v[4];
			for(int i=0;i<4;++i)v[i]=_mm_mul_ps(a[i].value,b.value);
			//0 4 8 C
			//1 5 9 D
			//2 6 A E
			//3 7 B F
			__m128 w0=::sandbox::sdk::detail::shuffle2<__m128,0,1,4,5>(v[0],v[1]);
			__m128 w1=::sandbox::sdk::detail::shuffle2<__m128,2,3,6,7>(v[0],v[1]);
			__m128 w2=::sandbox::sdk::detail::shuffle2<__m128,0,1,4,5>(v[2],v[3]);
			__m128 w3=::sandbox::sdk::detail::shuffle2<__m128,2,3,6,7>(v[2],v[3]);
			//0 2 8 A
			//1 3 9 B
			//4 6 C E
			//5 7 D F
			w0=_mm_add_ps(w0,w1);
			w2=_mm_add_ps(w2,w3);
			//02 8A
			//13 9B
			//46 CE
			//57 DF
			w1=::sandbox::sdk::detail::shuffle2<__m128,0,2,4,6>(w0,w2);
			w3=::sandbox::sdk::detail::shuffle2<__m128,1,3,5,7>(w0,w2);
			//02 13
			//46 57
			//8A 9B
			//CE DF
			return Vector4f(_mm_add_ps(w1,w3));
#endif
		}
	}
};
#endif //SANDBOX_SDK_MATH_MATRIX4F_HPP
